#include <intrins.h>
#include "Reg52.h"
#include "types.h"
#include "smi.h"
#include "uart.h"
#include "cmd.h"
#include "net.h"

#define	DISABLE_VLAN	(1 << 11)

void chip_soft_reset(int chip)
{
	uint16_t value;

	value = smi_read(chip, 0, 16);
	/* soft reset */
	value |= (1 << 12);
	smi_write(chip, 0, 16, value);
}

void vlan_init(int chip)
{
	uint16_t value;
	int i;

	/* enable vlan */
	value = smi_read(chip, 0, 16);
	value &= ~DISABLE_VLAN;
	smi_write(chip, 0, 16, value);

	/* clear vlan */
	for (i = VLAN_A; i < VLAN_NR; i++) {
		value = smi_read(chip, i, 24);
		value &= ~0x01FF;
		smi_write(chip, i, 24, value);
	}
}

void vlan_set_idx(int chip, int phy, int idx)
{
	uint16_t value;

	value = smi_read(chip, phy, 24) & 0x0FFF;
	value |= (idx << 12);
	smi_write(chip, phy, 24, value);
}

void vlan_set2(int chip, int vlan, int id, int member)
{
	uint16_t value;
	int i;

	/* set idx for each port */
	if (id) {
		for (i = 0; i < 9; i++) {
			if (member & (1 << i)) {
				vlan_set_idx(chip, i, vlan);
			}
		}
	}

	/* set membership */
	value = smi_read(chip, vlan, 24);
	value &= ~0x01FF;
	value |= (member & 0x01FF);
	smi_write(chip, vlan, 24, value);

//	chip_soft_reset(chip);
	return;
}

#if 0
static void vlan_set(void)
{
	uint16_t value;

	/* enable vlan */
	value = smi_read(RTL8309SCL, 0, 16);
	value &= ~DISABLE_VLAN;
	smi_write(RTL8309SCL, 0, 16, value);

#if 1
	value = 1 << 12 | 0x0F6A;
	smi_write(RTL8309SCL, 1, 24, value);
	value = 3 << 12 | 0x0F6A;
	smi_write(RTL8309SCL, 3, 24, value);
	value = 5 << 12 | 0x0F6A;
	smi_write(RTL8309SCL, 5, 24, value);
	value = 6 << 12 | 0x0F6A;
	smi_write(RTL8309SCL, 6, 24, value);
	chip_soft_reset(RTL8309SCL);
#endif
}
#endif

void vlan_default(void)
{
	/* chip 1 */
	vlan_set_idx(RTL8309SCR, 0, VLAN_A);
	vlan_set_idx(RTL8309SCR, 1, VLAN_B);
	vlan_set_idx(RTL8309SCR, 2, VLAN_C);
	vlan_set_idx(RTL8309SCR, 3, VLAN_D);
	vlan_set_idx(RTL8309SCR, 4, VLAN_E);
	vlan_set_idx(RTL8309SCR, 5, VLAN_F);
	vlan_set_idx(RTL8309SCR, 6, VLAN_G);
	vlan_set_idx(RTL8309SCR, 7, VLAN_H);

	vlan_set2(RTL8309SCR, VLAN_A, 0, PORT_N(0) | PORT_N(7));
	vlan_set2(RTL8309SCR, VLAN_B, 0, PORT_N(1) | PORT_N(7));
	vlan_set2(RTL8309SCR, VLAN_C, 0, PORT_N(2) | PORT_N(7));
	vlan_set2(RTL8309SCR, VLAN_D, 0, PORT_N(3) | PORT_N(7));
	vlan_set2(RTL8309SCR, VLAN_E, 0, PORT_N(4) | PORT_N(7));
	vlan_set2(RTL8309SCR, VLAN_F, 0, PORT_N(5) | PORT_N(7));
	vlan_set2(RTL8309SCR, VLAN_G, 0, PORT_N(6) | PORT_N(7));

	vlan_set2(RTL8309SCR, VLAN_H, 0,
		  PORT_N(0) | PORT_N(1) | PORT_N(2) | PORT_N(3) |
		  PORT_N(4) | PORT_N(5) | PORT_N(6) | PORT_N(7));

	/* chip 2 */
	vlan_set_idx(RTL8309SCL, 0, VLAN_A);
	vlan_set_idx(RTL8309SCL, 1, VLAN_B);
	vlan_set_idx(RTL8309SCL, 2, VLAN_C);
	vlan_set_idx(RTL8309SCL, 3, VLAN_D);
	vlan_set_idx(RTL8309SCL, 4, VLAN_E);
	vlan_set_idx(RTL8309SCL, 5, VLAN_F);
	vlan_set_idx(RTL8309SCL, 6, VLAN_G);
	vlan_set_idx(RTL8309SCL, 7, VLAN_H);

	vlan_set2(RTL8309SCL, VLAN_B, 0, PORT_N(1) | PORT_N(0));
	vlan_set2(RTL8309SCL, VLAN_C, 0, PORT_N(2) | PORT_N(0));
	vlan_set2(RTL8309SCL, VLAN_D, 0, PORT_N(3) | PORT_N(0));
	vlan_set2(RTL8309SCL, VLAN_E, 0, PORT_N(4) | PORT_N(0));
	vlan_set2(RTL8309SCL, VLAN_F, 0, PORT_N(5) | PORT_N(0));
	vlan_set2(RTL8309SCL, VLAN_G, 0, PORT_N(6) | PORT_N(0));
	vlan_set2(RTL8309SCL, VLAN_H, 0, PORT_N(7) | PORT_N(0));

	vlan_set2(RTL8309SCL, VLAN_A, 0,
		  PORT_N(0) | PORT_N(1) | PORT_N(2) | PORT_N(3) |
		  PORT_N(4) | PORT_N(5) | PORT_N(6) | PORT_N(7));

	/* chip 3 */
	vlan_set_idx(RTL8309SB, 0, VLAN_A);
	vlan_set_idx(RTL8309SB, 1, VLAN_B);
	vlan_set_idx(RTL8309SB, 2, VLAN_C);
	vlan_set_idx(RTL8309SB, 5, VLAN_D);
	vlan_set_idx(RTL8309SB, 7, VLAN_E);

	vlan_set2(RTL8309SCL, VLAN_A, 0, PORT_N(0) | PORT_N(2));
	vlan_set2(RTL8309SCL, VLAN_B, 0, PORT_N(1) | PORT_N(2));
	vlan_set2(RTL8309SCL, VLAN_D, 0, PORT_N(5) | PORT_N(2));
	vlan_set2(RTL8309SCL, VLAN_E, 0, PORT_N(7) | PORT_N(2));

	vlan_set2(RTL8309SCL, VLAN_C, 0,
		  PORT_N(0) | PORT_N(1) | PORT_N(2) |
		  PORT_N(5) | PORT_N(7));
}

#if 0
void show_reg(int chip, int phy, int reg)
{
	uint16_t value;

	value = smi_read(chip, phy, reg);
	uart_send_byte(value >> 8);
	uart_send_byte(value & 0xFF);
	uart_send_byte('\n');
}
#endif
